#include <stdlib.h>
#include <avr/interrupt.h>

#include "CoreC.h"


uint8_t SPIDataIn;
uint8_t SPIstartCountIn = 0;
uint8_t SPIcounterIn = 0;
uint8_t SPIArrayIn[8];
uint8_t SPIArrayOut[8];

void SPISlaveSetup()
{
  DDRB |= (1<<pinSPI_MISO); //set MISO pin as output
  SPCR |= _BV(SPE);  // turn on SPI in slave mode
  SPCR |= _BV(SPIE); // turn on interrupts
  SPCR &= ~(_BV(DORD)); //0=MSB
  SPCR &= ~(_BV(CPOL)); //0=Leading=Rising
  SPCR &= ~(_BV(CPHA)); //0=Sample on leading
  SPCR &= ~(_BV(MSTR)); //0=slave
  SPCR |= (_BV(SPR1)|_BV(SPR0));// 00=DIV4 01=DIV16 10=DIV64 11=DIV128 
  SPSR &= ~(_BV(SPI2X)); //1=SPIDIV*2
  
} 

ISR (SPI_STC_vect)
{
  PORTD ^= (PORTD&(1<<3)); // toggle
  SPIDataIn = SPDR;
  if ((!SPIstartCountIn) && (SPIDataIn == 0xFE) ) //keep in sync after lost bytes
  {
    SPIstartCountIn = 1;
    SPIArrayIn[0] = SPDR;
    SPDR = 0xFE;
  }
  else
  {
    if (SPIcounterIn)
    {
      SPIArrayIn[SPIcounterIn] = SPDR;
      SPDR = SPIArrayOut[SPIcounterIn];
    }

    if (SPIcounterIn == 5) //last byte - check and store data in then set data out
    {
      uint8_t check =0;
      for (uint8_t n = 0 ; n<5 ; n++) //0+1+2+3+4
      {
        check += SPIArrayIn[n];
      }
      
      if (SPIArrayIn[5]==check)
      {
        URobotDistance_type tmpURobotDistance;

        tmpURobotDistance.bytesLeft.b1 = SPIArrayIn[0];
        tmpURobotDistance.bytesLeft.b2 = SPIArrayIn[1];
        tmpURobotDistance.bytesRight.b1  = SPIArrayIn[2];
        tmpURobotDistance.bytesRight.b2  = SPIArrayIn[3];
        URobotDistance.robotDistance.LeftDistance += tmpURobotDistance.robotDistance.LeftDistance;
        URobotDistance.robotDistance.RightDistance += tmpURobotDistance.robotDistance.RightDistance;
        
        SPIArrayOut[0] = 0xFE;
        SPIArrayOut[1] = robotSpeed.LeftSpeed;
        SPIArrayOut[2] = robotSpeed.RightSpeed;
        SPIArrayOut[3] = robotSpeed.LeftSpeed + robotSpeed.RightSpeed;
      }//else throw it all away
      
      SPIcounterIn = 0;
      SPIstartCountIn = 0;
    }
  }
  if (SPIstartCountIn)//next byte
  {
    SPIcounterIn++;
  }
  
}

